using System;
using System.Collections.Generic;
using System.Text;
using System.Diagnostics;
using Microsoft.Xna.Framework;

namespace XnaDevRu.Bullet
{
	public class CompoundCollisionAlgorithm : CollisionAlgorithm
	{
		private List<CollisionAlgorithm> _childCollisionAlgorithms;
		private bool _isSwapped;

		public CompoundCollisionAlgorithm(
			CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo,
			CollisionObject bodyA,
			CollisionObject bodyB, bool isSwapped)
			: base(collisionAlgorithmConstructionInfo)
		{
			//Begin
			_isSwapped = isSwapped;

			CollisionObject collisionObject = isSwapped ? bodyB : bodyA;
			CollisionObject otherObject = isSwapped ? bodyA : bodyB;

			BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);

			CompoundShape compoundShape = collisionObject.CollisionShape as CompoundShape;
			int childrenNumber = compoundShape.ChildShapeCount;
			int index = 0;

			_childCollisionAlgorithms = new List<CollisionAlgorithm>(childrenNumber);

			for (index = 0; index < childrenNumber; index++)
			{
				CollisionShape childShape = compoundShape.GetChildShape(index);
				CollisionShape orgShape = collisionObject.CollisionShape;

				collisionObject.CollisionShape = childShape;
				_childCollisionAlgorithms[index] = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(collisionObject, otherObject);
				collisionObject.CollisionShape = orgShape;
			}
		}

		public override void ProcessCollision(
			CollisionObject bodyA,
			CollisionObject bodyB,
			DispatcherInfo dispatchInfo, ManifoldResult resultOut)
		{
			//Begin

			CollisionObject collisionObject = _isSwapped ? bodyB : bodyB;
			CollisionObject otherObject = _isSwapped ? bodyA : bodyB;

			//Debug.Assert(collisionObject.getCollisionShape().isCompound());
			BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);

			CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape;

			int childrenNumber = _childCollisionAlgorithms.Count;

			for (int i = 0; i < childrenNumber; i++)
			{
				CompoundShape childShape = compoundShape.GetChildShape(i) as CompoundShape;

				Matrix orgTransform = collisionObject.WorldTransform;
				CollisionShape orgShape = collisionObject.CollisionShape;

				Matrix childTransform = compoundShape.GetChildTransform(i);
				Matrix newChildWorld = orgTransform * childTransform;

				collisionObject.WorldTransform = newChildWorld;
				collisionObject.CollisionShape = childShape;
				_childCollisionAlgorithms[i].ProcessCollision(collisionObject, otherObject, dispatchInfo, resultOut);

				collisionObject.CollisionShape = orgShape;
				collisionObject.WorldTransform = orgTransform;
			}
		}

		public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
		{
			CollisionObject collisionObject = _isSwapped ? bodyB : bodyA;
			CollisionObject otherObject = _isSwapped ? bodyA : bodyB;

			BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);

			CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape;

			float hitFraction = 1.0f;

			for (int i = 0; i < _childCollisionAlgorithms.Count; i++)
			{
				CollisionShape childShape = compoundShape.GetChildShape(i);

				Matrix orgTransform = collisionObject.WorldTransform;
				CollisionShape orgShape = collisionObject.CollisionShape;

				Matrix childTransform = compoundShape.GetChildTransform(i);
				Matrix newChildWorld = orgTransform * childTransform;
				collisionObject.WorldTransform = newChildWorld;

				collisionObject.CollisionShape = childShape;
				float frac = _childCollisionAlgorithms[i].CalculateTimeOfImpact(
					collisionObject, otherObject, dispatchInfo, resultOut
				);

				if (frac < hitFraction)
				{
					hitFraction = frac;
				}

				collisionObject.CollisionShape = orgShape;
				collisionObject.WorldTransform = orgTransform;
			}

			return hitFraction;
		}

		public class CreateFunc : CollisionAlgorithmCreateFunction
		{
			public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
			{
				return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false);
			}
		};

		public class SwappedCreateFunc : CollisionAlgorithmCreateFunction
		{
			public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
			{
				return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true);
			}
		};
	}
}
